#include "Quaternion_to_Euler.h"
#include "icm20602.h"

void IMU_Quaternion_To_Euler(Q_info_Typedef *Q , IMU_Angle_t *Euler)
{
    float q0 = Q->q0;
    float q1 = Q->q1;
    float q2 = Q->q2;
    float q3 = Q->q3;

    Euler->Pitch = asinf(-2*q1*q3 + 2*q0*q2) * 180/My_PI; // pitch
    Euler->Roll = atan2f(2*q2*q3 + 2*q0*q1, -2*q1*q1 - 2*q2*q2 + 1) * 180/My_PI; // roll
    Euler->Yaw = atan2f(2*q1*q2 + 2*q0*q3, -2*q2*q2 - 2*q3*q3 + 1) * 180/My_PI; // yaw
    
}

void IMU_Euler_To_Quaternion(IMU_Angle_t *Euler , Q_info_Typedef *Q)
{
    float cy = arm_cos_f32((Euler->Yaw*0.5)*0.01745329252);
    float sy = arm_sin_f32((Euler->Yaw*0.5)*0.01745329252);
    float cp = arm_cos_f32((Euler->Pitch*0.5)*0.01745329252);
    float sp = arm_sin_f32((Euler->Pitch*0.5)*0.01745329252);
    float cr = arm_cos_f32((Euler->Roll*0.5)*0.01745329252);
    float sr = arm_sin_f32((Euler->Roll*0.5)*0.01745329252);

    Q->q0 = cy * cp * cr + sy * sp * sr;
    Q->q1 = cy * cp * sr - sy * sp * cr;
    Q->q2 = sy * cp * sr + cy * sp * cr;
    Q->q3 = sy * cp * cr - cy * sp * sr;

}
